﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using XDL.Framework.Collections;

namespace XDL.Framework.OpenSteer
{
    public class PathFinder
    {
        struct PathNode
        {
            public Vector2i Position;
            public bool IsValid;
            public int Parent;
            public int F;
            public int G;
            public int Tag;
            public int Cost;
            public PathNode(Vector2i position, bool isValid)
            {
                Position = position;
                IsValid = isValid;
                Parent = -1;
                F = G = 0;
                Tag = -1;
                Cost = 1;
            }

            public class Comparer :
                IComparer<PathNode>
            {
                public int Compare(PathNode x, PathNode y)
                {
                    if (x.F > y.F)
                        return 1;
                    else if (x.F < y.F)
                        return -1;
                    return 0;
                }
            }
        }


        PriorityQueueB<PathNode> m_open = new PriorityQueueB<PathNode>(new PathNode.Comparer());
        int m_width;
        int m_height;
        PathNode[] m_map;

        public PathFinder(IMap map)
        {
            Vector2i[] quadrants = {
                                          new Vector2i(1,0),
                                          new Vector2i(0,1),
                                          new Vector2i(-1,0),
                                          new Vector2i(0,-1),
                                          new Vector2i(1,-1),
                                          new Vector2i(-1,-1),
                                          new Vector2i(1,1),
                                          new Vector2i(-1,1)};

            m_width = map.Width;
            m_height = map.Height;
            m_map = new PathNode[map.Height * map.Width];
            for (int i = 0; i < m_width; i++)
            {
                for (int j = 0; j < m_height; j++)
                {
                    PathNode node = new PathNode(new Vector2i(i, j), map.IsWalkable(i, j));
                    // increase node cost near walls
                    foreach (Vector2i q in quadrants)
                        node.Cost += (map.IsWalkable(i + q.X, j + q.Y) ? 0 : 1);
                    m_map[i + j * m_width] = node;
                }
            }
        }

        int Vector2iToIndex(ref Vector2i pos)
        {
            return pos.X + pos.Y * m_width;
        }

        int Heuristic(Vector2i start, Vector2i end)
        {
            return (Math.Abs(start.X - end.X) +
                Math.Abs(start.Y - end.Y));
        }
        bool IsValid(Vector2i pos)
        {
            return pos.X < m_width && pos.X >= 0 && pos.Y < m_height && pos.Y >= 0;
        }

        static Vector2i[] g_quadrants = {
                                          new Vector2i(1,0),
                                          new Vector2i(0,1),
                                          new Vector2i(-1,0),
                                          new Vector2i(0,-1),
                                          new Vector2i(1,-1),
                                          new Vector2i(-1,-1),
                                          new Vector2i(1,1),
                                          new Vector2i(-1,1)
                                      };
        int m_openTagBase = 1;
        int m_closedTagBase = 2;
        public List<Vector2i> GetPath(Vector2i startPoint, Vector2i endPoint)
        {
            m_open.Clear();
            m_openTagBase += 2;
            m_closedTagBase += 2;
            int startIndex = Vector2iToIndex(ref startPoint);
            m_map[startIndex].G = 0;
            m_map[startIndex].F = 2;
            m_map[startIndex].Parent = -1; // make sure we can stop!!

            m_map[startIndex].Tag = m_openTagBase;
            m_open.Push(m_map[startIndex]);
            while (m_open.Count > 0)
            {
                // x := the node in openset having the lowest f_score[] value
                PathNode x = m_open.Pop();
                // if x = goal
                // return reconstruct_path(came_from,goal)
                if (x.Position == endPoint)
                    return ReconstructPath(x);

                // remove x from openset
                // add x to closedset
                int xIndex = Vector2iToIndex(ref x.Position);
                m_map[xIndex].Tag = m_closedTagBase;
                // foreach y in neighbor_nodes(x)
                foreach (Vector2i q in g_quadrants)
                {
                    Vector2i y = new Vector2i(x.Position.X + q.X, x.Position.Y + q.Y);
                    if (!IsValid(y))
                        continue;

                    int yIndex = Vector2iToIndex(ref y);
                    if (!m_map[yIndex].IsValid)
                        continue;

                    // if y in closedset
                    //   continue
                    if (m_map[yIndex].Tag == m_closedTagBase)
                        continue;

                    // tentative_g_score := g_score[x] + dist_between(x,y)
                    int tentative_g_score = x.G + x.Cost;
                    // if y not in openset
                    //   add y to openset
                    if (m_map[yIndex].Tag == m_openTagBase ||
                        m_map[yIndex].Tag == m_closedTagBase)
                    {
                        if (m_map[yIndex].G <= tentative_g_score)
                            continue;
                    }
                    // came_from[y] := x
                    m_map[yIndex].Parent = xIndex;
                    // g_score[y] := tentative_g_score
                    m_map[yIndex].G = tentative_g_score;
                    // f_score[y] := g_score[y] + h_score[y]
                    m_map[yIndex].F = tentative_g_score + Heuristic(y, endPoint);
                    //
                    m_map[yIndex].Tag = m_openTagBase;
                    m_open.Push(m_map[yIndex]);
                }
                //
                m_map[xIndex].Tag = m_closedTagBase;
            }
            return null;
        }

        List<Vector2i> ReconstructPath(PathNode node)
        {
            List<Vector2i> res = new List<Vector2i>();
            while (node.Parent != -1)
            {
                res.Insert(0, node.Position);
                // go back in the path
                node = m_map[node.Parent];
            }
            return res;
        }
    }
}
